import time, os, sys, struct
import math

from media.sensor import *
from media.display import *
from media.media import *
from machine import UART
from machine import FPIOA
from machine import TOUCH

# 配置引脚
fpioa = FPIOA()
fpioa.set_function(50, FPIOA.UART3_TXD)
fpioa.set_function(51, FPIOA.UART3_RXD)

# 初始化UART3，波特率115200，8位数据位，无校验，1位停止位
uart = UART(UART.UART3, baudrate=115200, bits=UART.EIGHTBITS, parity=UART.PARITY_NONE, stop=UART.STOPBITS_ONE)

sensor_id = 2
sensor = None

picture_width = 400
picture_height = 240

sendbuf = bytearray(9)
sendbuf[0] = 0x12
sendbuf[1] = 0x2c

angle_Flag=0 #角度预设
mission_Flag=3 #任务预设
Menu_Flag=0 #界面切换
last_press=0
press_state=0
# 显示模式选择：可以是 "VIRT"、"LCD" 或 "HDMI"
DISPLAY_MODE = "LCD"

# 根据模式设置显示宽高
if DISPLAY_MODE == "VIRT":
    DISPLAY_WIDTH = ALIGN_UP(1920, 16)
    DISPLAY_HEIGHT = 1080
elif DISPLAY_MODE == "LCD":
    DISPLAY_WIDTH = 800
    DISPLAY_HEIGHT = 480
elif DISPLAY_MODE == "HDMI":
    DISPLAY_WIDTH = 1920
    DISPLAY_HEIGHT = 1080
else:
    raise ValueError("未知的 DISPLAY_MODE，请选择 'VIRT', 'LCD' 或 'HDMI'")

def preprocess_img(img):
    img_bin = img.binary([(0, 18)], invert=True)
    # 去掉膨胀的代码反而识别更好了
    # img_bin = img_bin.dilate(1)
    return img_bin
def Touch_Mission(touch_obj):
    global mission_Flag, angle_Flag,press_state,last_press,Menu_Flag
    p=touch_obj.read(1)
    if(p!=()):
        press_state=0
    if(p==()):
        press_state=1
    if last_press!=press_state:
        for point in p:
            if point.x<160 and point.y>240 and point.y<320:
                mission_Flag+=1

            if point.x>160 and point.x<320 and point.y>240 and point.y<320:
                angle_Flag+=1
    last_press=press_state

def Menu_control():
    global mission_Flag, angle_Flag,Menu_Flag
    if Menu_Flag==0:
        str_value="M_Flag:%d"%mission_Flag
        menu.draw_rectangle(475,265,20,32,color=(255, 255, 0),thickness=1,fill=True)
        menu.draw_string_advanced(360, 260, 32, str_value, color=(255, 0, 0))
        str_value="A_Flag:%d"%angle_Flag
        menu.draw_rectangle(467,345,20,32,color=(255, 255, 0),thickness=1,fill=True)
        menu.draw_string_advanced(360, 340, 32, str_value, color=(255, 0, 0))
def angle_between(p1, p2, p3):
    # 计算p2点处，p1p2与p3p2的夹角（度）
    v1 = (p1[0] - p2[0], p1[1] - p2[1])
    v2 = (p3[0] - p2[0], p3[1] - p2[1])
    dot = v1[0] * v2[0] + v1[1] * v2[1]
    norm1 = math.sqrt(v1[0] ** 2 + v1[1] ** 2)
    norm2 = math.sqrt(v2[0] ** 2 + v2[1] ** 2)
    cos_angle = dot / (norm1 * norm2 + 1e-6)
    # 限定cos_angle范围防止数值误差
    cos_angle = max(-1, min(1, cos_angle))
    angle = math.acos(cos_angle) * 180 / math.pi
    return angle

def is_rectangular(corners, min_angle=80, max_angle=100):
    for i in range(4):
        angle = angle_between(corners[i - 1], corners[i], corners[(i + 1) % 4])
        if not (min_angle <= angle <= max_angle):
            return False
    return True

def is_a4_aspect_ratio(corners, min_ratio, max_ratio):
    # 计算矩形的外接矩形长宽比（A4纸宽:高约为0.707，或高:宽约1.414）
    xs = [pt[0] for pt in corners]
    ys = [pt[1] for pt in corners]
    width = max(ys) - min(ys)
    height = max(xs) - min(xs)
    if height == 0 or width == 0:
        return False
    ratio = float(height) / float(width) if height > width else float(width) / float(height)
    # 允许一定的误差带，比如1.35~1.45（A4严格为1.414）
    return min_ratio <= ratio <= max_ratio

try:
    # 构造一个具有默认配置的摄像头对象
    sensor = Sensor(id=sensor_id, width=1920, height=1080)
    # 重置摄像头sensor
    sensor.reset()

    # 无需进行镜像和翻转
    sensor.set_hmirror(False)
    sensor.set_vflip(False)

    sensor.set_framesize(width=picture_width, height=picture_height, chn=CAM_CHN_ID_0)
    sensor.set_pixformat(Sensor.RGB565, chn=CAM_CHN_ID_0)

    # 根据模式初始化显示器
    if DISPLAY_MODE == "VIRT":
        Display.init(Display.VIRT, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, fps=60)
    elif DISPLAY_MODE == "LCD":
        Display.init(Display.ST7701, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, to_ide=True)
    elif DISPLAY_MODE == "HDMI":
        Display.init(Display.LT9611, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, to_ide=True)

    # 初始化媒体管理器
    MediaManager.init()
    # 启动传感器
    sensor.run()
    clock = time.clock()
    fps = time.clock()
    tp=TOUCH(0)
    menu = image.Image(800, 480,image.RGB565)
    menu.draw_rectangle(0,0,800,480,color=(255,255,255),thickness=1,fill=True)
    menu.draw_rectangle(0,240,320,80,color=(0,0,0),thickness=2)
    menu.draw_string_advanced(120, 260, 32, "任务1", color=(255, 0, 0))
    menu.draw_rectangle(0,320,320,80,color=(0,0,0),thickness=2)
    menu.draw_string_advanced(40, 340, 32, "参数", color=(255, 0, 0))
    while True:
        fps.tick()
        os.exitpoint()
        # 捕获通道0的图像
        uart.write(sendbuf)
        sendbuf[6]=mission_Flag
        sendbuf[7]=angle_Flag
        sendbuf[8]=0

        img = sensor.snapshot(chn=CAM_CHN_ID_0)

        # 在屏幕左上角显示原始图像
        menu.draw_image(img,0,0)
        #Display.show_image(img, x=0, y=0, layer=Display.LAYER_OSD0)

        # --------图像预处理与矩形检测开始--------
        img_bin = preprocess_img(img)
        rects = img_bin.find_rects(threshold=30000)

        valid_rects = []
        centers = []
        for rect in rects:
            corner = rect.corners()


            # 计算面积
            xs = [pt[0] for pt in corner]
            ys = [pt[1] for pt in corner]
            width = max(xs) - min(xs)
            height = max(ys) - min(ys)
            area = width * height



            if len(corner) != 4:
                continue  # 只处理四边形
            if any(pt[0] < 0 or pt[1] < 0 for pt in corner):
                    continue
            if not is_rectangular(corner, min_angle=70, max_angle=110):
                continue  # 角度不合格，跳过
            if not is_a4_aspect_ratio(corner, min_ratio=1, max_ratio=1.5):
                continue  # 非A4长宽比，跳过

            if area < 2500:
                continue

            print(f"检测到矩形，面积为: {area}")
            # 绘制矩形
            img_bin.draw_line(corner[0][0], corner[0][1], corner[1][0], corner[1][1], color=(255, 0, 0), thickness=3)
            img_bin.draw_line(corner[1][0], corner[1][1], corner[2][0], corner[2][1], color=(255, 0, 0), thickness=3)
            img_bin.draw_line(corner[2][0], corner[2][1], corner[3][0], corner[3][1], color=(255, 0, 0), thickness=3)
            img_bin.draw_line(corner[3][0], corner[3][1], corner[0][0], corner[0][1], color=(255, 0, 0), thickness=3)
            valid_rects.append(rect)
            # 计算中心点
            center_x = sum(xs) / 4
            center_y = sum(ys) / 4
            centers.append((center_x, center_y))

        # 画面中心点坐标
        frame_center_x = picture_width / 2
        frame_center_y = picture_height / 2

        print("画面中心点坐标：({:.2f}, {:.2f})".format(frame_center_x, frame_center_y))

        # 检查是否检测到至少一个合格的矩形
        if len(valid_rects) >= 1:
            # 选最大面积的矩形
            max_area = 0
            max_idx = 0
            for i, rect in enumerate(valid_rects):
                xs = [pt[0] for pt in rect.corners()]
                ys = [pt[1] for pt in rect.corners()]
                width = max(xs) - min(xs)
                height = max(ys) - min(ys)
                area = width * height
                if area > max_area:
                    max_area = area
                    max_idx = i
            target_center = centers[max_idx]
            print(f"检测到目标矩形中心坐标: {target_center}")
            sendbuf[2] = (int(target_center[0]) >> 8) & 0xff
            sendbuf[3] = int(target_center[0]) & 0xff
            sendbuf[4] = (int(target_center[1]) >> 8) & 0xff
            sendbuf[5] = int(target_center[1]) & 0xff
            sendbuf[8] = 1
            # 标注目标中心点为黑色小圆点
            img_bin.draw_circle(int(target_center[0]), int(target_center[1]), 2, color=(0, 0, 0), thickness=2)
            # ---- 串口发送 ----


        # 标注画面中心点为黑色小圆点
        img_bin.draw_circle(int(frame_center_x), int(frame_center_y), 2, color=(0, 0, 0), thickness=2)

        img_bin.draw_string_advanced(0, 0, 20, "fps: {}".format(clock.fps()), color=(0, 255, 0))

        # 合成一个新的显示缓冲区，左上显示原始图像，右上显示处理后图像
        img.draw_image(img_bin, picture_width - img_bin.width(), 0)
        # --------图像处理结束--------
        Touch_Mission(tp)
        Menu_control()

        print(sendbuf)
        # 在屏幕右上角显示处理后的图像
        menu.draw_image(img, DISPLAY_WIDTH-picture_width,0)
        #Display.show_image(img, x=DISPLAY_WIDTH-picture_width, y=0, layer=Display.LAYER_OSD1)
        Display.show_image(menu,0,0);
except KeyboardInterrupt as e:
    print("用户停止: ", e)
except BaseException as e:
    print(f"异常: {e}")
finally:
    # 停止传感器运行
    if isinstance(sensor, Sensor):
        sensor.stop()
    # 反初始化显示模块
    Display.deinit()
    os.exitpoint(os.EXITPOINT_ENABLE_SLEEP)
    time.sleep_ms(100)
    # 释放媒体缓冲区
    MediaManager.deinit()
